/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, 2013, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * Author: Eitan Marder-Eppstein
 *         David V. Lu!!
 *********************************************************************/
#include <algorithm>
#include <costmap_2d/inflation_layer.h>
#include <costmap_2d/costmap_math.h>
#include <costmap_2d/footprint.h>
#include <boost/thread.hpp>
#include <pluginlib/class_list_macros.h>

PLUGINLIB_EXPORT_CLASS(costmap_2d::InflationLayer, costmap_2d::Layer)

using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::INSCRIBED_INFLATED_OBSTACLE;
using costmap_2d::NO_INFORMATION;

namespace costmap_2d
{
InflationLayer::InflationLayer()
  : resolution_(0)
  , inflation_radius_(0)
  , inscribed_radius_(0)
  , weight_(0)
  , inflate_unknown_(false)
  , cell_inflation_radius_(0)
  , cached_cell_inflation_radius_(0)
  , dsrv_(NULL)
  , seen_(NULL)
  , cached_costs_(NULL)
  , cached_distances_(NULL)
  , last_min_x_(-std::numeric_limits<float>::max())
  , last_min_y_(-std::numeric_limits<float>::max())
  , last_max_x_(std::numeric_limits<float>::max())
  , last_max_y_(std::numeric_limits<float>::max())
{
  inflation_access_ = new boost::recursive_mutex();
}

void InflationLayer::onInitialize()
{
  {
    boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
    ros::NodeHandle nh("~/" + name_), g_nh;
    current_ = true;
    if (seen_)
      delete[] seen_;
    seen_ = NULL;
    seen_size_ = 0;
    need_reinflation_ = false;
    // 配置了动态参数服务的回调函数
    dynamic_reconfigure::Server<costmap_2d::InflationPluginConfig>::CallbackType cb = boost::bind(&InflationLayer::reconfigureCB, this, _1, _2);

    if (dsrv_ != NULL){
      dsrv_->clearCallback();
      dsrv_->setCallback(cb);
    }
    else{
      dsrv_ = new dynamic_reconfigure::Server<costmap_2d::InflationPluginConfig>(ros::NodeHandle("~/" + name_));
      dsrv_->setCallback(cb);
    }
  }
  //调用matchSize
  matchSize();
}

void InflationLayer::reconfigureCB(costmap_2d::InflationPluginConfig &config, uint32_t level){
  setInflationParameters(config.inflation_radius, config.cost_scaling_factor);
  if (enabled_ != config.enabled || inflate_unknown_ != config.inflate_unknown) {
    enabled_ = config.enabled;
    inflate_unknown_ = config.inflate_unknown;
    need_reinflation_ = true;
  }
}

void InflationLayer::matchSize(){
  boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
  costmap_2d::Costmap2D* costmap = layered_costmap_->getCostmap();
  resolution_ = costmap->getResolution();
  cell_inflation_radius_ = cellDistance(inflation_radius_);
  computeCaches();  // 计算了两个buffer，这两个二维buffer直接存储了[i，j]的distance 和cost

  unsigned int size_x = costmap->getSizeInCellsX(), size_y = costmap->getSizeInCellsY();
  if (seen_)
    delete[] seen_;
  seen_size_ = size_x * size_y;
  seen_ = new bool[seen_size_];
}

// need_reinflation_默认false，更新bound这里和其他两层的主要区别是，膨胀层在传入的bound的值的基础上，
// 通过inflation_radius_再次扩张，函数内部基本什么都没做，因此会维持上一层地图调用updateBounds的值。
void InflationLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x, double* max_y){
  if (need_reinflation_){
    last_min_x_ = *min_x;
    last_min_y_ = *min_y;
    last_max_x_ = *max_x;
    last_max_y_ = *max_y;
    // For some reason when I make these -<double>::max() it does not
    // work with Costmap2D::worldToMapEnforceBounds(), so I'm using
    // -<float>::max() instead.
    *min_x = -std::numeric_limits<float>::max();
    *min_y = -std::numeric_limits<float>::max();
    *max_x = std::numeric_limits<float>::max();
    *max_y = std::numeric_limits<float>::max();
    need_reinflation_ = false;
  }
  else{
    double tmp_min_x = last_min_x_;
    double tmp_min_y = last_min_y_;
    double tmp_max_x = last_max_x_;
    double tmp_max_y = last_max_y_;
    last_min_x_ = *min_x;
    last_min_y_ = *min_y;
    last_max_x_ = *max_x;
    last_max_y_ = *max_y;
    *min_x = std::min(tmp_min_x, *min_x) - inflation_radius_;
    *min_y = std::min(tmp_min_y, *min_y) - inflation_radius_;
    *max_x = std::max(tmp_max_x, *max_x) + inflation_radius_;
    *max_y = std::max(tmp_max_y, *max_y) + inflation_radius_;
  }
}

// 该函数会去调用computeCaches(); 更新两个两个维度为[cell_inflation_radius_+2][cell_inflation_radius_+2] 的二维buffer：cached_costs_，cached_distances_
void InflationLayer::onFootprintChanged(){
  inscribed_radius_ = layered_costmap_->getInscribedRadius();
  cell_inflation_radius_ = cellDistance(inflation_radius_);
  computeCaches();
  need_reinflation_ = true;

  ROS_DEBUG("InflationLayer::onFootprintChanged(): num footprint points: %lu," " inscribed_radius_ = %.3f, inflation_radius_ = %.3f", layered_costmap_->getFootprint().size(), inscribed_radius_, inflation_radius_);
}

// 用指针master_array指向主地图，并获取主地图的尺寸，确认seen_数组被正确设置  该函数实现了inflation 具体是如何操作完成的膨胀的工作
void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j){
  boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
  if (cell_inflation_radius_ == 0)
    return;

  // ASSERT 定义在ros/assert.h中
  // ROS_BREAK() : 用于中断程序并输出本句所在文件/行数.
  // ROS_ASSERT(cond) : 检验条件是否成功,若失败则终端程序并输出文件/行数/条件.
  // ROS_ASSERT_MSG(cond,...): 检验条件是否成立,并在失败时输出自定的额外信息.
  // ROS_ASSERT_CMD(cond,cmd): 检查条件是否满足,不满足就执行对于的cmd但不退出

  // 确保膨胀区列表在周期开始时为空（应始终为真）   https://zhuanlan.zhihu.com/p/369081317
  ROS_ASSERT_MSG(inflation_cells_.empty(), "The inflation list must be empty at the beginning of inflation");

  unsigned char* master_array = master_grid.getCharMap();
  unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY();

  if (seen_ == NULL) {
    ROS_WARN("InflationLayer::updateCosts(): seen_ array is NULL");
    seen_size_ = size_x * size_y;
    seen_ = new bool[seen_size_];// 这里分配了一个size 和master map 尺寸一样的bool 数组
  }
  else if (seen_size_ != size_x * size_y) {
    ROS_WARN("InflationLayer::updateCosts(): seen_ array size is wrong");
    delete[] seen_;
    seen_size_ = size_x * size_y;
    seen_ = new bool[seen_size_];
  }
  memset(seen_, false, size_x * size_y * sizeof(bool));

  // We need to include in the inflation cells outside the bounding
  // box min_i...max_j, by the amount cell_inflation_radius_.  Cells
  // up to that distance outside the box can still influence the costs
  // stored in cells inside the box.
  // 边缘膨胀：由于（min_i，min_j，max_i，max_j）是由上一层的地图已经在updateBounds中更新过，
  // 而InflationLayer层并未去改变它。因此这里的操作是将传入的boudns，按照机器人的膨胀尺寸，扩张这个bounds
  min_i -= cell_inflation_radius_;
  min_j -= cell_inflation_radius_;
  max_i += cell_inflation_radius_;
  max_j += cell_inflation_radius_;

  min_i = std::max(0, min_i);
  min_j = std::max(0, min_j);
  max_i = std::min(int(size_x), max_i);
  max_j = std::min(int(size_y), max_j);

  // 膨胀区列表，我们将要访问的单元格添加到与其到最近障碍物的距离相关联的列表中，使用 map<distance, list> 来模拟之前使用的优先级队列，从而显著提升性能
  // 从致命障碍开始：根据定义，距离为 0.0
  // 下面的两重循环完成将master map中的LETHAL_OBSTACLE cell的下标index，i，j传给函数
  // inline void InflationLayer::enqueue(unsigned char* grid, unsigned int index, unsigned int mx, unsigned int my,unsigned int src_x, unsigned int src_y)
  // 注意这个函数的参数，因此这里传给enqueue的信息为：将master map中，目标cell索引为（index，mx，my），最近的障碍物索引为（src_x,src_y）的。
  std::vector<CellData>& obs_bin = inflation_cells_[0.0];
  for (int j = min_j; j < max_j; j++) {
    for (int i = min_i; i < max_i; i++) {
      int index = master_grid.getIndex(i, j);
      unsigned char cost = master_array[index];
      if (cost == LETHAL_OBSTACLE) {
        obs_bin.push_back(CellData(index, i, j, i, j));  // 已经把所有障碍物的单元格找到了，并都被放到obs_bin这个vector中了
      }
    }
  }

  // 通过增加距离来处理单元格； 新单元格被附加到相应的距离箱中，因此它们可以超越先前插入但更远的单元格
  std::map<double, std::vector<CellData> >::iterator bin;
  for (bin = inflation_cells_.begin(); bin != inflation_cells_.end(); ++bin) {
    for (int i = 0; i < bin->second.size(); ++i) {
      const CellData& cell = bin->second[i];
      unsigned int index = cell.index_;
      if(seen_[index]){   //如果被访问过，就忽略该单元格
        continue;
      }

      seen_[index] = true;

      unsigned int mx = cell.x_;
      unsigned int my = cell.y_;
      unsigned int sx = cell.src_x_;
      unsigned int sy = cell.src_y_;

      // assign the cost associated with the distance from an obstacle to the cell
      unsigned char cost = costLookup(mx, my, sx, sy);
      unsigned char old_cost = master_array[index];
      if (old_cost == NO_INFORMATION && (inflate_unknown_ ? (cost > FREE_SPACE) : (cost >= INSCRIBED_INFLATED_OBSTACLE)))
        master_array[index] = cost;
      else
        master_array[index] = std::max(old_cost, cost);

      // 尝试将当前单元格的邻居放入膨胀列表
      // 上面首先从priority queue中弹出最大distance的cell，再将这个cell的前后左右四个cell都塞进inflation_queue_。
      // 由于下面关于enqueue的调用，最后两个参数(sx, sy)没有改变，所以保证了这个索引一定是obstacle cell。
      // 由于在 enqueue入口会检查 if (!seen_[index])，这保证了这些cell不会被重复的塞进去。由于这是一个priority queue，
      // 因此障碍物的周边点，每次都是离障碍物最远的点被弹出，并被检查，这样保证了这种扩张是朝着离障碍物远的方向进行。
      if (mx > 0)
        enqueue(index - 1, mx - 1, my, sx, sy);
      if (my > 0)
        enqueue(index - size_x, mx, my - 1, sx, sy);
      if (mx < size_x - 1)
        enqueue(index + 1, mx + 1, my, sx, sy);
      if (my < size_y - 1)
        enqueue(index + size_x, mx, my + 1, sx, sy);
    }
  }
  inflation_cells_.clear();
}

// 给定代价图grid中的一个单元格的索引index，将其放入等待障碍物膨胀的列表中，enqueue函数首先检查当前cell(mx, my)到障碍物cell(src_x, src_y)的distance，
// 通过查表即可distanceLookup，如果距离小于cell_inflation_radius_，则检查cost，通过查表即可costLookup
// 然后用cost更新master map中当前cell的值。最后将当前的cell 打包成CellData类型，并塞到inflation_queue_
inline void InflationLayer::enqueue(unsigned int index, unsigned int mx, unsigned int my, unsigned int src_x, unsigned int src_y){
  if (!seen_[index]){  // 避免cell被重复的塞进来
    double distance = distanceLookup(mx, my, src_x, src_y);//计算距离比膨胀半径更远一个单元格，因此我们可以在下面进行检查
    if (distance > cell_inflation_radius_) // 保证了只处理在cell_inflation_radius_范围内的cell
      return;
    inflation_cells_[distance].push_back(CellData(index, mx, my, src_x, src_y)); // 将单元格数据推入通胀列表并标记
  }
}

void InflationLayer::computeCaches() {                                        //计算缓存(Caches)值，经过这个函数之后，膨胀半径和每个单元格的代价值都准备好了
  if (cell_inflation_radius_ == 0)
    return;

  if (cell_inflation_radius_ != cached_cell_inflation_radius_) {              // 基于膨胀半径，计算cached_distances_和cached_costs_，两者为下面膨胀计算的参照物
    deleteKernels();
    cached_costs_ = new unsigned char*[cell_inflation_radius_ + 2];           // cached_distances_和cached_costs_的行数都是cell_inflation_radius_+2
    cached_distances_ = new double*[cell_inflation_radius_ + 2];
    for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i) {
      cached_costs_[i] = new unsigned char[cell_inflation_radius_ + 2];       // cached_distances_和cached_costs_的列数也是cell_inflation_radius_+2
      cached_distances_[i] = new double[cell_inflation_radius_ + 2];
      for (unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j) {
        cached_distances_[i][j] = hypot(i, j);
      }//cached_distances_ 每个元素的值到(0,0)点的距离，这个(0,0)点并不是地图的(0,0)点，而是相对的概念，后面通过 distanceLookup 函数得到当前单元格相对障碍物的距离，进而计算出到障碍物的代价值
    }
    cached_cell_inflation_radius_ = cell_inflation_radius_;                   // 设置cached_cell_inflation_radius_，这个if内程序不会再次进入
  }

  for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i) {
    for (unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j) {
      cached_costs_[i][j] = computeCost(cached_distances_[i][j]);
    }
  }
}

void InflationLayer::deleteKernels(){
  if (cached_distances_ != NULL) {
    for (unsigned int i = 0; i <= cached_cell_inflation_radius_ + 1; ++i) {
      if (cached_distances_[i])
        delete[] cached_distances_[i];
    }
    if (cached_distances_)
      delete[] cached_distances_;
    cached_distances_ = NULL;
  }

  if (cached_costs_ != NULL) {
    for (unsigned int i = 0; i <= cached_cell_inflation_radius_ + 1; ++i) {
      if (cached_costs_[i])
        delete[] cached_costs_[i];
    }
    delete[] cached_costs_;
    cached_costs_ = NULL;
  }
}

void InflationLayer::setInflationParameters(double inflation_radius, double cost_scaling_factor)
{
  if (weight_ != cost_scaling_factor || inflation_radius_ != inflation_radius)
  {
    // Lock here so that reconfiguring the inflation radius doesn't cause segfaults
    // when accessing the cached arrays
    boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);

    inflation_radius_ = inflation_radius;
    cell_inflation_radius_ = cellDistance(inflation_radius_);
    weight_ = cost_scaling_factor;
    need_reinflation_ = true;
    computeCaches();
  }
}

}  // namespace costmap_2d
